home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / interpolation / cspline.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-04-18  |  12.0 KB  |  454 lines
  1. /* interpolation/cspline.c
  2.  * 
  3.  * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. /* Author:  G. Jungman
  21.  */
  22. #include <config.h>
  23. #include <stdlib.h>
  24. #include <gsl/gsl_errno.h>
  25. #include <gsl/gsl_linalg.h>
  26. #include <gsl/gsl_vector.h>
  27. #include "integ_eval.h"
  28. #include <gsl/gsl_interp.h>
  29.  
  30. typedef struct
  31. {
  32.   double * c;
  33.   double * g;
  34.   double * diag;
  35.   double * offdiag;
  36. } cspline_state_t;
  37.  
  38.  
  39. /* common initialization */
  40. static void *
  41. cspline_alloc (size_t size)
  42. {
  43.   cspline_state_t * state = (cspline_state_t *) malloc (sizeof (cspline_state_t));
  44.  
  45.   if (state == NULL)
  46.     {
  47.       GSL_ERROR_NULL("failed to allocate space for state", GSL_ENOMEM);
  48.     }
  49.   
  50.   state->c = (double *) malloc (size * sizeof (double));
  51.   
  52.   if (state->c == NULL)
  53.     {
  54.       free (state);
  55.       GSL_ERROR_NULL("failed to allocate space for c", GSL_ENOMEM);
  56.     }
  57.  
  58.   state->g = (double *) malloc (size * sizeof (double));
  59.   
  60.   if (state->g == NULL)
  61.     {
  62.       free (state->c);
  63.       free (state);
  64.       GSL_ERROR_NULL("failed to allocate space for g", GSL_ENOMEM);
  65.     }
  66.  
  67.   state->diag = (double *) malloc (size * sizeof (double));
  68.   
  69.   if (state->diag == NULL)
  70.     {
  71.       free (state->g);
  72.       free (state->c);
  73.       free (state);
  74.       GSL_ERROR_NULL("failed to allocate space for diag", GSL_ENOMEM);
  75.     }
  76.  
  77.   state->offdiag = (double *) malloc (size * sizeof (double));
  78.   
  79.   if (state->offdiag == NULL)
  80.     {
  81.       free (state->diag);
  82.       free (state->g);
  83.       free (state->c);
  84.       free (state);
  85.       GSL_ERROR_NULL("failed to allocate space for offdiag", GSL_ENOMEM);
  86.     }
  87.  
  88.   return state;
  89. }
  90.  
  91.  
  92. /* natural spline calculation
  93.  * see [Engeln-Mullges + Uhlig, p. 254]
  94.  */
  95. static int
  96. cspline_init (void * vstate, const double xa[], const double ya[],
  97.               size_t size)
  98. {
  99.   cspline_state_t *state = (cspline_state_t *) vstate;
  100.  
  101.   size_t i;
  102.   size_t num_points = size;
  103.   size_t max_index = num_points - 1;    /* Engeln-Mullges + Uhlig "n" */
  104.   size_t sys_size = max_index - 1;   /* linear system is sys_size x sys_size */
  105.   
  106.   state->c[0] = 0.0;
  107.   state->c[max_index] = 0.0;
  108.   
  109.   for (i = 0; i < sys_size; i++)
  110.     {
  111.       const double h_i   = xa[i + 1] - xa[i];
  112.       const double h_ip1 = xa[i + 2] - xa[i + 1];
  113.       const double ydiff_i   = ya[i + 1] - ya[i];
  114.       const double ydiff_ip1 = ya[i + 2] - ya[i + 1];
  115.       state->offdiag[i] = h_ip1;
  116.       state->diag[i] = 2.0 * (h_ip1 + h_i);
  117.       state->g[i] = 3.0 * (ydiff_ip1 / h_ip1  -  ydiff_i / h_i);
  118.     }
  119.   
  120.   {
  121.     gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
  122.     gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
  123.     gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size);
  124.     gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);
  125.     
  126.     int status = gsl_linalg_solve_symm_tridiag(&diag_vec.vector, 
  127.                                                &offdiag_vec.vector, 
  128.                                                &g_vec.vector, 
  129.                                                &solution_vec.vector);
  130.     return status;
  131.   }
  132. }
  133.  
  134.  
  135. /* periodic spline calculation
  136.  * see [Engeln-Mullges + Uhlig, p. 256]
  137.  */
  138. static int
  139. cspline_init_periodic (void * vstate, const double xa[], const double ya[],
  140.                        size_t size)
  141. {
  142.   cspline_state_t *state = (cspline_state_t *) vstate;
  143.  
  144.   size_t i;
  145.   size_t num_points = size;
  146.   size_t max_index = num_points - 1;  /* Engeln-Mullges + Uhlig "n" */
  147.   size_t sys_size = max_index;    /* linear system is sys_size x sys_size */
  148.  
  149.   if (sys_size == 2) {
  150.     /* solve 2x2 system */
  151.     
  152.     const double h0 = xa[1] - xa[0];
  153.     const double h1 = xa[2] - xa[1];
  154.     const double h2 = xa[3] - xa[2];
  155.     const double A = 2.0*(h0 + h1);
  156.     const double B = h0 + h1;
  157.     double g[2];
  158.     double det;
  159.     
  160.     g[0] = 3.0 * ((ya[2] - ya[1]) / h1 - (ya[1] - ya[0]) / h0);
  161.     g[1] = 3.0 * ((ya[1] - ya[2]) / h2 - (ya[2] - ya[1]) / h1);
  162.     
  163.     det = 3.0 * (h0 + h1) * (h0 + h1);
  164.     state->c[1] = ( A * g[0] - B * g[1])/det;
  165.     state->c[2] = (-B * g[0] + A * g[1])/det;
  166.     state->c[0] = state->c[2];
  167.     
  168.     return GSL_SUCCESS;
  169.   } else {
  170.     
  171.     state->offdiag[max_index] =  xa[1]-xa[0];
  172.     
  173.     for (i = 0; i < sys_size; i++) {
  174.       const double h_i   = xa[i + 1] - xa[i];
  175.       const double h_ip1 = xa[(i + 2) % num_points] - xa[i + 1];
  176.       const double ydiff_i   = ya[i + 1] - ya[i];
  177.       const double ydiff_ip1 = ya[(i + 2) % num_points] - ya[i + 1];
  178.       state->offdiag[i] = h_ip1;
  179.       state->diag[i] = 2.0 * (h_ip1 + h_i);
  180.       state->g[i] = 3.0 * (ydiff_ip1 / h_ip1 - ydiff_i / h_i);
  181.     }
  182.     
  183.     {
  184.       gsl_vector_view g_vec = gsl_vector_view_array(state->g, sys_size);
  185.       gsl_vector_view diag_vec = gsl_vector_view_array(state->diag, sys_size);
  186.       gsl_vector_view offdiag_vec = gsl_vector_view_array(state->offdiag, sys_size);
  187.       gsl_vector_view solution_vec = gsl_vector_view_array ((state->c) + 1, sys_size);
  188.       
  189.       int status = gsl_linalg_solve_symm_cyc_tridiag(&diag_vec.vector, 
  190.                                                      &offdiag_vec.vector, 
  191.                                                      &g_vec.vector, 
  192.                                                      &solution_vec.vector);
  193.       state->c[0] = state->c[max_index];
  194.       
  195.       return status;
  196.     }
  197.   }
  198. }
  199.  
  200.  
  201. static
  202. void
  203. cspline_free (void * vstate)
  204. {
  205.   cspline_state_t *state = (cspline_state_t *) vstate;
  206.   
  207.   free (state->c);
  208.   free (state->g);
  209.   free (state->diag);
  210.   free (state->offdiag);
  211.   free (state);
  212. }
  213.  
  214. /* function for common coefficient determination
  215.  */
  216. static inline void
  217. coeff_calc (const double c_array[], double dy, double dx, size_t index,  
  218.             double * b, double * c, double * d)
  219. {
  220.   const double c_i = c_array[index];
  221.   const double c_ip1 = c_array[index + 1];
  222.   *b = (dy / dx) - dx * (c_ip1 + 2.0 * c_i) / 3.0;
  223.   *c = c_i;
  224.   *d = (c_ip1 - c_i) / (3.0 * dx);
  225. }
  226.  
  227.  
  228. static
  229. int
  230. cspline_eval (const void * vstate,
  231.               const double x_array[], const double y_array[], size_t size,
  232.               double x,
  233.               gsl_interp_accel * a,
  234.               double *y)
  235. {
  236.   const cspline_state_t *state = (const cspline_state_t *) vstate;
  237.  
  238.   double x_lo, x_hi;
  239.   double dx;
  240.   size_t index;
  241.   
  242.   if (a != 0)
  243.     {
  244.       index = gsl_interp_accel_find (a, x_array, size, x);
  245.     }
  246.   else
  247.     {
  248.       index = gsl_interp_bsearch (x_array, x, 0, size - 1);
  249.     }
  250.   
  251.   /* evaluate */
  252.   x_hi = x_array[index + 1];
  253.   x_lo = x_array[index];
  254.   dx = x_hi - x_lo;
  255.   if (dx > 0.0)
  256.     {
  257.       const double y_lo = y_array[index];
  258.       const double y_hi = y_array[index + 1];
  259.       const double dy = y_hi - y_lo;
  260.       double delx = x - x_lo;
  261.       double b_i, c_i, d_i; 
  262.       coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
  263.       *y = y_lo + delx * (b_i + delx * (c_i + delx * d_i));
  264.       return GSL_SUCCESS;
  265.     }
  266.   else
  267.     {
  268.       *y = 0.0;
  269.       return GSL_EINVAL;
  270.     }
  271. }
  272.  
  273.  
  274. static
  275. int
  276. cspline_eval_deriv (const void * vstate,
  277.                     const double x_array[], const double y_array[], size_t size,
  278.                     double x,
  279.                     gsl_interp_accel * a,
  280.                     double *dydx)
  281. {
  282.   const cspline_state_t *state = (const cspline_state_t *) vstate;
  283.  
  284.   double x_lo, x_hi;
  285.   double dx;
  286.   size_t index;
  287.   
  288.   if (a != 0)
  289.     {
  290.       index = gsl_interp_accel_find (a, x_array, size, x);
  291.     }
  292.   else
  293.     {
  294.       index = gsl_interp_bsearch (x_array, x, 0, size - 1);
  295.     }
  296.   
  297.   /* evaluate */
  298.   x_hi = x_array[index + 1];
  299.   x_lo = x_array[index];
  300.   dx = x_hi - x_lo;
  301.   if (dx > 0.0)
  302.     {
  303.       const double y_lo = y_array[index];
  304.       const double y_hi = y_array[index + 1];
  305.       const double dy = y_hi - y_lo;
  306.       double delx = x - x_lo;
  307.       double b_i, c_i, d_i; 
  308.       coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
  309.       *dydx = b_i + delx * (2.0 * c_i + 3.0 * d_i * delx);
  310.       return GSL_SUCCESS;
  311.     }
  312.   else
  313.     {
  314.       *dydx = 0.0;
  315.       return GSL_FAILURE;
  316.     }
  317. }
  318.  
  319.  
  320. static
  321. int
  322. cspline_eval_deriv2 (const void * vstate,
  323.                      const double x_array[], const double y_array[], size_t size,
  324.                      double x,
  325.                      gsl_interp_accel * a,
  326.                      double * y_pp)
  327. {
  328.   const cspline_state_t *state = (const cspline_state_t *) vstate;
  329.  
  330.   double x_lo, x_hi;
  331.   double dx;
  332.   size_t index;
  333.   
  334.   if (a != 0)
  335.     {
  336.       index = gsl_interp_accel_find (a, x_array, size, x);
  337.     }
  338.   else
  339.     {
  340.       index = gsl_interp_bsearch (x_array, x, 0, size - 1);
  341.     }
  342.   
  343.   /* evaluate */
  344.   x_hi = x_array[index + 1];
  345.   x_lo = x_array[index];
  346.   dx = x_hi - x_lo;
  347.   if (dx > 0.0)
  348.     {
  349.       const double y_lo = y_array[index];
  350.       const double y_hi = y_array[index + 1];
  351.       const double dy = y_hi - y_lo;
  352.       double delx = x - x_lo;
  353.       double b_i, c_i, d_i;
  354.       coeff_calc(state->c, dy, dx, index,  &b_i, &c_i, &d_i);
  355.       *y_pp = 2.0 * c_i + 6.0 * d_i * delx;
  356.       return GSL_SUCCESS;
  357.     }
  358.   else
  359.     {
  360.       *y_pp = 0.0;
  361.       return GSL_FAILURE;
  362.     }
  363. }
  364.  
  365.  
  366. static
  367. int
  368. cspline_eval_integ (const void * vstate,
  369.                     const double x_array[], const double y_array[], size_t size,
  370.                     gsl_interp_accel * acc,
  371.                     double a, double b,
  372.                     double * result)
  373. {
  374.   const cspline_state_t *state = (const cspline_state_t *) vstate;
  375.  
  376.   size_t i, index_a, index_b;
  377.   
  378.   if (acc != 0)
  379.     {
  380.       index_a = gsl_interp_accel_find (acc, x_array, size, a);
  381.       index_b = gsl_interp_accel_find (acc, x_array, size, b);
  382.     }
  383.   else
  384.     {
  385.       index_a = gsl_interp_bsearch (x_array, a, 0, size - 1);
  386.       index_b = gsl_interp_bsearch (x_array, b, 0, size - 1);
  387.     }
  388.  
  389.   *result = 0.0;
  390.   
  391.   /* interior intervals */
  392.   for(i=index_a; i<=index_b; i++) {
  393.     const double x_hi = x_array[i + 1];
  394.     const double x_lo = x_array[i];
  395.     const double y_lo = y_array[i];
  396.     const double y_hi = y_array[i + 1];
  397.     const double dx = x_hi - x_lo;
  398.     const double dy = y_hi - y_lo;
  399.     if(dx != 0.0) {
  400.       double b_i, c_i, d_i; 
  401.       coeff_calc(state->c, dy, dx, i,  &b_i, &c_i, &d_i);
  402.       
  403.       if (i == index_a || i == index_b)
  404.         {
  405.           double x1 = (i == index_a) ? a : x_lo;
  406.           double x2 = (i == index_b) ? b : x_hi;
  407.           *result += integ_eval(y_lo, b_i, c_i, d_i, x_lo, x1, x2);
  408.         }
  409.       else
  410.         {
  411.           *result += dx * (y_lo + dx*(0.5*b_i + dx*(c_i/3.0 + 0.25*d_i*dx)));
  412.         }
  413.     }
  414.     else {
  415.       *result = 0.0;
  416.       return GSL_FAILURE;
  417.     }
  418.   }
  419.   
  420.   return GSL_SUCCESS;
  421. }
  422.  
  423. static const gsl_interp_type cspline_type = 
  424. {
  425.   "cspline", 
  426.   2,
  427.   &cspline_alloc,
  428.   &cspline_init,
  429.   &cspline_eval,
  430.   &cspline_eval_deriv,
  431.   &cspline_eval_deriv2,
  432.   &cspline_eval_integ,
  433.   &cspline_free
  434. };
  435.  
  436. const gsl_interp_type * gsl_interp_cspline = &cspline_type;
  437.  
  438. static const gsl_interp_type cspline_periodic_type = 
  439. {
  440.   "cspline-periodic", 
  441.   2,
  442.   &cspline_alloc,
  443.   &cspline_init_periodic,
  444.   &cspline_eval,
  445.   &cspline_eval_deriv,
  446.   &cspline_eval_deriv2,
  447.   &cspline_eval_integ,
  448.   &cspline_free
  449. };
  450.  
  451. const gsl_interp_type * gsl_interp_cspline_periodic = &cspline_periodic_type;
  452.  
  453.  
  454.